#include "app_type.h"
#include "compiler.h"
#include "interfaces.h"

#if IFS_USBD_EN

#include "M45x_USBD.h"
#include "core.h"
#include "M451Series.h"

#define M45X_USBD_EP_REG(ep, reg)\
	*((__IO uint32_t *)((uint32_t)&USBD->reg + (uint32_t)((uint8_t)(ep) * 0x10)))

#define M45X_USBD_EP_NUM				8
#define M45X_USBD_BUFFER_SIZE			512
#define M45X_USBD_BUFFER_ADDR			(USBD_BASE + 0x100)
static int16_t EP_Cfg_Ptr;

struct interface_usbd_callback_t m45x_usbd_callback;

#define M45X_USBD_EPIN					0x10
#define M45X_USBD_EPOUT					0x00
static int8_t m45x_usbd_epaddr[M45X_USBD_EP_NUM];
static int16_t m45x_usbd_epsize[M45X_USBD_EP_NUM];


vsf_err_t m45x_usbd_init(uint32_t int_priority)
{
	struct m45x_info_t *info;
	m45x_unlock_reg();
	// Enable IP clock
	CLK->CLKDIV0 &= ~CLK_CLKDIV0_USBDIV_Msk;
	info = m45x_interface_get_info();
	CLK->CLKDIV0 |= info->pll_freq_hz / 48000000 - 1;
	CLK->APBCLK |= CLK_APBCLK0_USBDCKEN_Msk;
	m45x_lock_reg();

	USBD->INTEN = USB_INTEN_BUSEVT | USB_INTEN_USBEVT | USB_INTEN_FLDET | USB_INTEN_WAKEUP;
	NVIC_EnableIRQ(USBD_IRQn);

	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_fini(void)
{
	USBD->INTEN = 0;
	NVIC_DisableIRQ(USBD_IRQn);

	m45x_unlock_reg();
	// Enable IP clock
	CLK->APBCLK &= ~CLK_APBCLK0_USBDCKEN_Msk;
	m45x_lock_reg();

	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_reset(void)
{
	return VSFERR_NONE;
}

void USB_Istr(void);
vsf_err_t m45x_usbd_poll(void)
{
	USB_Istr();
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_connect(void)
{
	USBD->SE0 &= ~USBD_SE0_SE0_Msk;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_disconnect(void)
{
	USBD->SE0 |= USBD_SE0_SE0_Msk;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_set_address(uint8_t address)
{
	USBD->FADDR = address;
	return VSFERR_NONE;
}

uint8_t m45x_usbd_get_address(void)
{
	return USBD->FADDR;
}

vsf_err_t m45x_usbd_suspend(void)
{
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_resume(void)
{
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_lowpower(uint8_t level)
{
	return VSFERR_NONE;
}

uint32_t m45x_usbd_get_frame_number(void)
{
	return 0;
}

vsf_err_t m45x_usbd_get_setup(uint8_t *buffer)
{
	uint8_t *prt;
	prt = (uint8_t *)(M45X_USBD_BUFFER_ADDR + (USBD->BUFSEG & 0x000001f8));

	buffer[0] = prt[0];
	buffer[1] = prt[1];
	buffer[2] = prt[2];
	buffer[3] = prt[3];
	buffer[4] = prt[4];
	buffer[5] = prt[5];
	buffer[6] = prt[6];
	buffer[7] = prt[7];
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_prepare_buffer(void)
{
	uint16_t i;
	EP_Cfg_Ptr = 8;
	for (i = 0; i < M45X_USBD_EP_NUM; i++)
	{
		m45x_usbd_epaddr[i] = -1;
		m45x_usbd_epsize[i] = -1;
	}
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_reset(uint8_t idx)
{
	return VSFERR_NONE;
}

static int8_t m45x_usbd_ep(uint8_t idx)
{
	uint8_t i;

	for (i = 0; i < sizeof(m45x_usbd_epaddr); i++)
	{
		if ((int8_t)idx == m45x_usbd_epaddr[i])
		{
			return (int8_t)i;
		}
	}
	return -1;
}

static int8_t m45x_usbd_get_free_ep(uint8_t idx)
{
	uint8_t i;

	for (i = 0; i < sizeof(m45x_usbd_epaddr); i++)
	{
		if (-1 == m45x_usbd_epaddr[i])
		{
			m45x_usbd_epaddr[i] = (int8_t)idx;
			return i;
		}
	}
	return -1;
}

vsf_err_t m45x_usbd_ep_set_IN_dbuffer(uint8_t idx)
{
	return VSFERR_NONE;
}

bool m45x_usbd_ep_is_IN_dbuffer(uint8_t idx)
{
	return false;
}

vsf_err_t m45x_usbd_ep_switch_IN_buffer(uint8_t idx)
{
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_set_IN_epsize(uint8_t idx, uint16_t epsize)
{
	int8_t index, index_out;

	index_out = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	index = m45x_usbd_get_free_ep(idx | M45X_USBD_EPIN);
	if (index < 0)
	{
		return VSFERR_FAIL;
	}

	if (index_out < 0)
	{
		if ((EP_Cfg_Ptr + epsize) > M45X_USBD_BUFFER_SIZE)
		{
			return VSFERR_NOT_ENOUGH_RESOURCES;
		}
		M45X_USBD_EP_REG(index, BUFSEG0) = EP_Cfg_Ptr;
		EP_Cfg_Ptr += (epsize & 0x7) ? ((epsize & 0xfff8) + 8) : epsize;
	}
	else
	{
		M45X_USBD_EP_REG(index, BUFSEG0) = M45X_USBD_EP_REG(index_out, BUFSEG0);
	}
	m45x_usbd_epsize[index] = epsize;
	M45X_USBD_EP_REG(index, CFG0) = USB_CFG_EPMODE_IN | (idx & 0x0f);
	return VSFERR_NONE;
}

uint16_t m45x_usbd_ep_get_IN_epsize(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return 0;
	}

	return m45x_usbd_epsize[index];
}

vsf_err_t m45x_usbd_ep_set_IN_stall(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}
	M45X_USBD_EP_REG(index, CFG0) |= USB_CFG_SSTALL;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_clear_IN_stall(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, CFG0) &= ~USB_CFG_SSTALL;
	M45X_USBD_EP_REG(index, CFG0) |= USB_CFG_CSTALL;
	return VSFERR_NONE;
}

bool m45x_usbd_ep_is_IN_stall(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return true;
	}

	return (M45X_USBD_EP_REG(index, CFG0) & USB_CFG_SSTALL) > 0;
}

vsf_err_t m45x_usbd_ep_reset_IN_toggle(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, CFG0) &= ~USB_CFG_DSQ_SYNC_DATA1;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_toggle_IN_toggle(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	if (M45X_USBD_EP_REG(index, CFG0) & USB_CFG_DSQ_SYNC_DATA1)
	{
		M45X_USBD_EP_REG(index, CFG0) &= ~USB_CFG_DSQ_SYNC_DATA1;
	}
	else
	{
		M45X_USBD_EP_REG(index, CFG0) |= USB_CFG_DSQ_SYNC_DATA1;
	}
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_set_IN_count(uint8_t idx, uint16_t size)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, MXPLD0) = size;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_write_IN_buffer(uint8_t idx, uint8_t *buffer,
											uint16_t size)
{
	int8_t index;
	uint8_t *prt;
	uint16_t i;

	index = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM) ||
		(m45x_usbd_ep_get_IN_epsize(idx) < size))
	{
		return VSFERR_FAIL;
	}

	prt = (uint8_t *)(M45X_USBD_BUFFER_ADDR +
						(M45X_USBD_EP_REG(index, BUFSEG0) & 0x000001f8));
	for (i = 0; i  < size; i++)
	{
		*prt++ = *buffer++;
	}
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_set_OUT_dbuffer(uint8_t idx)
{
	return VSFERR_NONE;
}

bool m45x_usbd_ep_is_OUT_dbuffer(uint8_t idx)
{
	return false;
}

vsf_err_t m45x_usbd_ep_switch_OUT_buffer(uint8_t idx)
{
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_set_OUT_epsize(uint8_t idx, uint16_t epsize)
{
	int8_t index, index_in;

	index_in = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	index = m45x_usbd_get_free_ep(idx | M45X_USBD_EPOUT);
	if (index < 0)
	{
		return VSFERR_FAIL;
	}

	if (index_in < 0)
	{
		if ((EP_Cfg_Ptr + epsize) > M45X_USBD_BUFFER_SIZE)
		{
			return VSFERR_NOT_ENOUGH_RESOURCES;
		}
		M45X_USBD_EP_REG(index, BUFSEG0) = EP_Cfg_Ptr;
		EP_Cfg_Ptr += (epsize & 0x7) ? ((epsize & 0xfff8) + 8) : epsize;
	}
	else
	{
		M45X_USBD_EP_REG(index, BUFSEG0) = M45X_USBD_EP_REG(index_in, BUFSEG0);
	}
	m45x_usbd_epsize[index] = epsize;
	M45X_USBD_EP_REG(index, CFG0) = USB_CFG_EPMODE_OUT | (idx & 0x0f);
	return VSFERR_NONE;
}

uint16_t m45x_usbd_ep_get_OUT_epsize(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return 0;
	}

	return m45x_usbd_epsize[index];
}

vsf_err_t m45x_usbd_ep_set_OUT_stall(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, CFG0) |= USB_CFG_SSTALL;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_clear_OUT_stall(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, CFG0) &= ~USB_CFG_SSTALL;
	M45X_USBD_EP_REG(index, CFG0) |= USB_CFG_CSTALL;
	return VSFERR_NONE;
}

bool m45x_usbd_ep_is_OUT_stall(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return true;
	}

	return (M45X_USBD_EP_REG(index, CFG0) & USB_CFG_SSTALL) > 0;
}

vsf_err_t m45x_usbd_ep_reset_OUT_toggle(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, CFG0) &= ~USB_CFG_DSQ_SYNC_DATA1;
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_toggle_OUT_toggle(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	if (M45X_USBD_EP_REG(index, CFG0) & USB_CFG_DSQ_SYNC_DATA1)
	{
		M45X_USBD_EP_REG(index, CFG0) &= ~USB_CFG_DSQ_SYNC_DATA1;
	}
	else
	{
		M45X_USBD_EP_REG(index, CFG0) |= USB_CFG_DSQ_SYNC_DATA1;
	}
	return VSFERR_NONE;
}

uint16_t m45x_usbd_ep_get_OUT_count(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return 0;
	}

	return M45X_USBD_EP_REG(index, MXPLD0);
}

vsf_err_t m45x_usbd_ep_read_OUT_buffer(uint8_t idx, uint8_t *buffer,
											uint16_t size)
{
	int8_t index;
	uint8_t *prt;
	uint16_t i;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM) ||
		(m45x_usbd_ep_get_OUT_count(idx) < size))
	{
		return VSFERR_FAIL;
	}

	prt = (uint8_t *)(M45X_USBD_BUFFER_ADDR +
						(M45X_USBD_EP_REG(index, BUFSEG0) & 0x000001f8));
	for (i = 0; i  < size; i++)
	{
		*buffer++ = *prt++;
	}
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_enable_OUT(uint8_t idx)
{
	int8_t index;

	index = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index < 0) || (index >= M45X_USBD_EP_NUM))
	{
		return VSFERR_FAIL;
	}

	M45X_USBD_EP_REG(index, MXPLD0) = m45x_usbd_epsize[index];
	return VSFERR_NONE;
}

vsf_err_t m45x_usbd_ep_set_type(uint8_t idx, enum interface_usbd_eptype_t type)
{
	int8_t index_in, index_out;
	uint8_t isoch;

	index_in = m45x_usbd_ep(idx | M45X_USBD_EPIN);
	index_out = m45x_usbd_ep(idx | M45X_USBD_EPOUT);
	if ((index_in < 0) && (index_out < 0))
	{
		return VSFERR_FAIL;
	}

	switch (type)
	{
	case USB_EP_TYPE_CONTROL:
	case USB_EP_TYPE_INTERRUPT:
	case USB_EP_TYPE_BULK:
		isoch = 0;
		break;
	case USB_EP_TYPE_ISO:
		isoch = 0x10;
		break;
	default:
		return VSFERR_INVALID_PARAMETER;
	}

	if (index_in >= 0)
	{
		M45X_USBD_EP_REG(index_in, CFG0) |= isoch | USB_CFG_CSTALL;
	}
	if (index_out >= 0)
	{
		M45X_USBD_EP_REG(index_out, CFG0) |= isoch | USB_CFG_CSTALL;
	}
	return VSFERR_NONE;
}

void USB_Istr(void)
{
	static uint32_t intsts;

	intsts = USBD->INTSTS;
	intsts &= USBD->INTEN | 0xfffffff0;

	if (!intsts)
	{
		return;
	}

	if (intsts & USB_INTSTS_WKEUP_STS)
	{
		if (m45x_usbd_callback.on_wakeup != NULL)
		{
			m45x_usbd_callback.on_wakeup(m45x_usbd_callback.param);
		}
		USBD->INTSTS |= USB_INTSTS_WKEUP_STS;
	}

	if (intsts & USB_INTSTS_FLD_STS)
	{
		// TODO
		USBD->INTSTS |= USB_INTSTS_FLD_STS;
	}

	if (intsts & USB_INTSTS_BUS_STS)
	{
		uint32_t bussts = USBD->BUSSTS;
		if (bussts & USB_BUSSTS_TIMEOUT)
		{
			if (m45x_usbd_callback.on_error != NULL)
			{
				m45x_usbd_callback.on_error(m45x_usbd_callback.param,
												USBERR_ERROR);
			}
		}
		if (bussts & USB_BUSSTS_SUSPEND)
		{
			if (m45x_usbd_callback.on_suspend != NULL)
			{
				m45x_usbd_callback.on_suspend(m45x_usbd_callback.param);
			}
		}
		if (bussts & USB_BUSSTS_RESUME)
		{
			// TODO
		}
		if (bussts & USB_BUSSTS_USBRST)
		{
			if (m45x_usbd_callback.on_reset != NULL)
			{
				m45x_usbd_callback.on_reset(m45x_usbd_callback.param);
			}
		}
		USBD->INTSTS |= USB_INTSTS_BUS_STS;
	}

	if (intsts & USB_INTSTS_USB_STS)
	{
		uint8_t i;
		if (intsts & USB_INTSTS_SETUP)
		{
			if (m45x_usbd_callback.on_setup != NULL)
			{
				// TODO
				M45X_USBD_EP_REG(0, CFG0) |= USB_CFG_DSQ_SYNC_DATA1;
				M45X_USBD_EP_REG(1, CFG0) |= USB_CFG_DSQ_SYNC_DATA1;
				m45x_usbd_callback.on_setup(m45x_usbd_callback.param);
			}
			USBD->INTSTS |= USB_INTSTS_SETUP;
		}

		for (i = 0; i < 8; i++)
		{
			if (intsts & (0x00010000 << i))
			{
				uint8_t epsts;
				if (i < 6)
				{
					epsts = (USBD->EPSTS & (0x00000f00 << (i * 4))) >>
							(i * 4 + 8);
				}
				else
				{
					epsts = (USBD->EPSTS2 & (0x0000000f << ((i - 6) * 4))) >>
							((i - 6) * 4);
				}
				switch (epsts)
				{
				case 0:	// IN ACK
					if (m45x_usbd_callback.on_in != NULL)
					{
						m45x_usbd_callback.on_in(\
												m45x_usbd_callback.param,
												m45x_usbd_epaddr[i] & 0xf);
					}
					break;
				case 2:	// OUT DATA0 ACK
				case 6:	// OUT DATA1 ACK
					if (m45x_usbd_callback.on_out != NULL)
					{
						m45x_usbd_callback.on_out(\
												m45x_usbd_callback.param,
												m45x_usbd_epaddr[i] & 0xf);
					}
					break;
				case 3:	// SETUP ACK
					break;
				case 7:	// ISO ACK
					break;
				}
				USBD->INTSTS |= 0x00010000 << i;
			}
		}
	}
}

ROOTFUNC void USBD_IRQHandler(void)
{
	USB_Istr();
}

#endif
